/*#include "inc/hw_types.h"		// tBoolean
#include "inc/hw_memmap.h"
#include "RASLib/init.h"
#include "driverlib/gpio.h"
#include "driverlib/sysctl.h"

#include "RASLib/motor.h"
//#include "RASLib/encoder.h"
#include "RASLib/servo.h"

void initMotors(void);
void initLineSensor(void);
void initUART(void);
void initIRSensor(void);
//void initEncoders(void);
void Wait(long ms);
void initServo(void);
void SetDischargeTime(int ms);
long getADCValue(void);
unsigned char ReadLineSensor(void);
void UARTprintf(const char *pcString, ...);
long PIDwallfollow(long position);	
void turnOne(void);	

long distance_WF;
long Kp_WF=15,Ki_WF=0,Kd_WF=27;//d
long currentError_WF,lastError_WF;
long integral_WF=0;	
char turn=0;
   

void wallfollow(void){
	//Encoders
	//signed long encoder0, encoder1;

	signed char speed=127;
	signed char motorOffset=14;

	//Random variables 	  
	long MV;
	long MVabs=0;
	signed char velocity;
	unsigned char servoPosition=20;
	
	//wallfollow values
	//char lineSensorByte;
	long IRvalue = 0;

	LockoutProtection();
	InitializeMCU();
	initIRSensor();
//	initEncoders();
	initMotors();
	initLineSensor();
	initUART();
	initServo();
	SetDischargeTime(220);
//	PresetEncoderCounts(0,0);

	SetServoPosition(SERVO_0, servoPosition);
	Wait(2000);			   
	SetMotorPowers(speed,speed-motorOffset);
	distance_WF=getADCValue();
	//UARTprintf("%d", (distance_WF));
	
	while(1){	
		 
		IRvalue = getADCValue();			  
		UARTprintf("%d    %d \r",IRvalue, distance_WF);
		if(IRvalue>=distance_WF + noWall){ //+ noWall){	 //> + noWall
			turn=1;
			
		}
		if(turn==1){
			 turnOne();
		}	 */
		/*if(IRvalue<= 250){
			UARTprintf("\noWall		%d", IRvalue);
			SetMotorPowers(75,127);
		}
		else{
			UARTprintf("\nwall		%d", IRvalue);
			SetMotorPowers(127,127);
		}*/
		
		/*
		if(IRvalue <= 200){
		 	turn = 1;
		} 
		if(turn ==1){
			UARTprintf("no wall");
		 	//SetMotorPowers(75, 127);
			SetMotorPowers(0, 0);
			Wait(10000);
			SetMotorPowers(speed, speed - motorOffset);
			turn = 2;
		}
		
		
		MV = PIDwallfollow(IRvalue);
		if(MV>255)MV=255;
		if(MV*-1>255)MV=-255;
		//If MV is positive, the robot needs to go right. If MV is negative, the robot needs to go left
		//But... we first need to come up with a linear relationship relating MV to the speed of the
		//reduced wheel (This means an mx+b relationship... more like a b-mx relationship)
		MVabs=MV;
		if(MV<0)MVabs*=-1; //
		velocity=(signed char)(speed-MVabs);
		if(velocity<-128)velocity=-128;	
		if(MV>0){		   //
			SetMotorPowers(velocity,speed-motorOffset);
		}
		else if(MV<0){	  //
			
			SetMotorPowers(speed,velocity);
		}
		else{//MV equals 0; never going to happen, but might?
			SetMotorPowers(speed,speed-motorOffset);
		}  
	}
}

void turnOne(void){
	//SetMotorPowers(127,75);
	SetMotorPowers(0,0);
	Wait(1500);
	turn=0;
	//SetMotorPowers(speed,speed-motorOffset);
	//distance_WF = getADCValue();
}

long PIDwallfollow(long position){
	//PID variables 	 
	signed long derivative=0;
    signed long Pout;
	signed long Iout;
	signed long Dout;
	signed long MV=0;
	
	lastError_WF=currentError_WF;
	//Now it's time to compute the error...
	currentError_WF = (distance_WF - (position))/20;
	//Now that we have the error, we can update the derivative and integral
	derivative=currentError_WF-lastError_WF;
	integral_WF+=currentError_WF;
	//It is now time to compute Pout, Iout, and Dout, thus getting MV (the manipulated variable)
	//It should be noted that a positive derivative means the robot needs to turn left, and a
	//negative derivative means the robot needs to turn right. A positive integral means the
	//robot needs to skew left, and a negative integral value means the robot needs to skew right
	Pout=Kp_WF*currentError_WF;								   
	Dout=Kd_WF*derivative;
	Iout=Ki_WF*integral_WF/50;
	MV=Pout+Iout+Dout;
	
	return MV;	 
}					  */
